{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "a756d64a",
   "metadata": {},
   "source": [
    "## Start the 3D Visualizer and the droidlet dashboard"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1825de31",
   "metadata": {},
   "outputs": [],
   "source": [
    "from droidlet.dashboard.o3dviz import O3DViz\n",
    "# if False, opens a native window for the 3D visualization. \n",
    "# If True, opens a web-based viewer for the 3D stuff that is available at port http://localhost:8889\n",
    "web_streaming = False \n",
    "o3dviz = O3DViz(web_streaming)\n",
    "o3dviz.start()\n",
    "\n",
    "from droidlet import dashboard\n",
    "dashboard.start()\n",
    "# this has to come after the `dashboard.start` function above\n",
    "from droidlet.event import sio"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f909e7f9",
   "metadata": {},
   "source": [
    "## Some useful imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b669bc9d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import time\n",
    "import cv2\n",
    "from matplotlib import pyplot as plt\n",
    "import open3d as o3d\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9dd7932",
   "metadata": {},
   "source": [
    "## Import and connect to the HelloRobotMover"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3f0b7370",
   "metadata": {},
   "outputs": [],
   "source": [
    "from droidlet.lowlevel.hello_robot.hello_robot_mover import HelloRobotMover"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "57fa9097",
   "metadata": {},
   "outputs": [],
   "source": [
    "mover = HelloRobotMover(ip=\"100.95.90.42\") # ip of the robot\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "959815db",
   "metadata": {},
   "source": [
    "## Get the rgb, depth and globally-registered point-clouds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c1b3091f",
   "metadata": {},
   "outputs": [],
   "source": [
    "rgb_depth = mover.get_rgb_depth()\n",
    "img = rgb_depth.rgb\n",
    "plt.imshow(img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2728c31",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.imshow(rgb_depth.depth)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bef955db",
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_open3d_pcd(rgb_depth):\n",
    "    points, colors = rgb_depth.ptcloud.reshape(-1, 3), rgb_depth.rgb.reshape(-1, 3)\n",
    "    colors = colors / 255.\n",
    "    pcd = o3d.geometry.PointCloud()\n",
    "    pcd.points = o3d.utility.Vector3dVector(points)\n",
    "    pcd.colors = o3d.utility.Vector3dVector(colors)\n",
    "    return pcd\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "909ba15d",
   "metadata": {},
   "outputs": [],
   "source": [
    "point_cloud = get_open3d_pcd(rgb_depth)\n",
    "\n",
    "o3dviz.put('pointcloud', point_cloud)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "19d76a11",
   "metadata": {},
   "source": [
    "## Get the SLAM obstacle map and base state in droidlet canonical co-ordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5f05a6d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "obstacle_map = mover.get_obstacles_in_canonical_coords()\n",
    "base_state = mover.get_base_pos_in_canonical_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "de608551",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Plot them\n",
    "x, y = [m[0] for m in obstacle_map], [m[1] for m in obstacle_map]\n",
    "\n",
    "\n",
    "plt.plot(y, x, 'o', markersize=1)\n",
    "plt.plot(base_state[0], base_state[1], 'ro', markersize=12)\n",
    "\n",
    "xorigin, yorigin = 0., 0.\n",
    "newx = (base_state[0] - xorigin) * math.cos(base_state[2] * math.pi / 180)\n",
    "newy = (base_state[1] - yorigin) * math.sin(base_state[2] * math.pi / 180)\n",
    "plt.plot([base_state[0], newx], [base_state[1], newy], 'b')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ce461514",
   "metadata": {},
   "outputs": [],
   "source": [
    "# you can also plot it into the dashboard\n",
    "x, y, yaw = base_state\n",
    "sio.emit(\"map\",\n",
    "    {\"x\": x, \"y\": y, \"yaw\": yaw, \"map\": obstacle_map},\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16aea403",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "7178391b",
   "metadata": {},
   "source": [
    "## Move the robot using it's navigation system"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b195a213",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Move forward by 1 metre (x, y, yaw)\n",
    "mover.move_relative([1.0, 0.0, 0.0], blocking=False)\n",
    "\n",
    "# turn the robot\n",
    "mover.turn(-math.radians(20))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f35ac20b",
   "metadata": {},
   "source": [
    "## Move the robot directly using it's base API"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8f40f74",
   "metadata": {},
   "outputs": [],
   "source": [
    "mover.bot.go_to_relative([1.0, 0.0, 0.0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "473ed00f",
   "metadata": {},
   "source": [
    "## Move it's camera"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8968ab9c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set the camera's tilt and pan\n",
    "mover.bot.set_tilt(math.radians(-60) )\n",
    "mover.bot.set_pan(math.radians(-30) )"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0564b106",
   "metadata": {},
   "source": [
    "## Stop the robot's actions immediately"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fab0cf71",
   "metadata": {},
   "outputs": [],
   "source": [
    "mover.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "99cfcd34",
   "metadata": {},
   "source": [
    "## Access mover's backend services directly via the RPC classes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9144f17e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# mover.bot\n",
    "# mover.slam\n",
    "# mover.nav\n",
    "# mover.cam"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a019eda9",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
